/*
 * SPDX-FileCopyrightText: 2021 Espressif Systems (Shanghai) CO LTD
 *
 * SPDX-License-Identifier: Unlicense OR CC0-1.0
 */

/****************************************************************************
*
* This demo showcases creating a GATT database using a predefined attribute table.
* It acts as a GATT server and can send adv data, be connected by client.
* Run the gatt_client demo, the client demo will automatically connect to the gatt_server_service_table demo.
* Client demo will enable GATT server's notify after connection. The two devices will then exchange
* data.
*
****************************************************************************/

//include 文件

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"
#include "esp_system.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "esp_bt.h"

#include "esp_gap_ble_api.h"
#include "esp_gatts_api.h"
#include "esp_bt_main.h"
#include "gatts_table_creat_demo.h"

#include <stdio.h>
#include "driver/gpio.h"
#include "rom/gpio.h"
#include "driver/ledc.h"
#include "esp_err.h"
#include "ctype.h"
#include <string.h>
#include <sys/param.h>
#include "lwip/err.h"
#include "lwip/sockets.h"
#include "lwip/sys.h"
#include <lwip/netdb.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <driver/ledc.h>
#include "esp_log.h"
//esp32 numbers for uart1
#include "driver/uart.h"
#include "esp_vfs_dev.h"

#include "esp_gatt_common_api.h"

//define 区
#define GATTS_TABLE_TAG "GATTS_TABLE_JUNGTS_LED" //蓝牙标签

#define PROFILE_NUM                 1
#define PROFILE_APP_IDX             0
#define ESP_APP_ID                  0x55
#define SAMPLE_DEVICE_NAME          "JUNGTS_LED_v1"
#define SVC_INST_ID                 0

/* The max length of characteristic value. When the GATT client performs a write or prepare write operation,
*  the data length must be less than GATTS_DEMO_CHAR_VAL_LEN_MAX.
*/
#define GATTS_DEMO_CHAR_VAL_LEN_MAX 500     
#define PREPARE_BUF_MAX_SIZE        1024                // buffer最大长度
#define CHAR_DECLARATION_SIZE       (sizeof(uint8_t))   // 描述符长度

#define ADV_CONFIG_FLAG             (1 << 0)
#define SCAN_RSP_CONFIG_FLAG        (1 << 1)


// 定义舵机控制参数
#define SERVO1_GPIO             GPIO_NUM_23  // 舵机 1 信号引脚
#define SERVO2_GPIO             GPIO_NUM_22  // 舵机 2 信号引脚
#define LEDC_TIMER              LEDC_TIMER_0 // LEDC 定时器
#define LEDC_MODE               LEDC_LOW_SPEED_MODE
#define LEDC_CHANNEL1           LEDC_CHANNEL_0 // 舵机 1 的 LEDC 通道
#define LEDC_CHANNEL2           LEDC_CHANNEL_1 // 舵机 2 的 LEDC 通道
#define LEDC_DUTY_RES           LEDC_TIMER_16_BIT // 16 位分辨率
#define LEDC_FREQUENCY          50 // 50Hz

// 定义舵机角度范围
#define SERVO_MIN_ANGLE         0.0f   // 0°
#define SERVO_MAX_ANGLE         180.0f // 180°

// 定义 PWM 值范围
#define SERVO_MIN_DUTY          1638  // 0° 对应的 PWM 值（16 位分辨率）
#define SERVO_MAX_DUTY          8192  // 180° 对应的 PWM 值（16 位分辨率）


// 定义缓慢移动的步长和间隔时间
#define STEP_SIZE               1 // 每次角度变化的步长（0.1°）
#define STEP_DELAY_MS           1    // 每次角度变化的间隔时间（毫秒）


// 定义uart参数
#define UART_NUM UART_NUM_2
#define UART_TXD GPIO_NUM_17
#define UART_RXD GPIO_NUM_16
#define UART_BAUD_RATE 115200


// 接收缓冲区大小
#define BUFFER_SIZE 256

// 创建互斥锁确保线程安全
SemaphoreHandle_t uart_mutex;

// 接收缓冲区
uint8_t rx_buffer[BUFFER_SIZE];
uint8_t tx_buffer[BUFFER_SIZE];
size_t rx_len = 0;
size_t tx_len = 0;

QueueHandle_t uart_event_queue;

// 辅助函数：打印数据为十六进制格式
void print_hex(const uint8_t *data, size_t len) {
    for (size_t i = 0; i < len; i++) {
        ESP_LOGI(GATTS_TABLE_TAG, "%02X ", data[i]);
    }
}

// 串口接收任务
static void uart_event_task(void *pvParameters) {
    uart_event_t event;
    static uint8_t dtmp[BUFFER_SIZE]; // 将 dtmp 设为静态变量

    while (true) {
        if (xQueueReceive(uart_event_queue, (void *)&event, portMAX_DELAY)) {
            switch (event.type) {
                case UART_DATA:
                    size_t bytes_to_read = event.size;
                    if (bytes_to_read > BUFFER_SIZE) {
                        bytes_to_read = BUFFER_SIZE; // 防止溢出
                    }

                    // 读取数据到临时缓冲区
                    int read_bytes = uart_read_bytes(UART_NUM, dtmp, bytes_to_read, portMAX_DELAY);
                    if (read_bytes > 0) {
                        xSemaphoreTake(uart_mutex, portMAX_DELAY);
                        memcpy(rx_buffer + rx_len, dtmp, read_bytes);
                        rx_len += read_bytes;
                        xSemaphoreGive(uart_mutex);

                        // 打印接收到的数据提示信息
                        ESP_LOGI(GATTS_TABLE_TAG, "Received from UART:");

                        // 打印接收到的数据为十六进制格式
                        print_hex(dtmp, read_bytes); // 打印实际读取的数据长度

                        // 打印接收到的数据长度
                        ESP_LOGI(GATTS_TABLE_TAG, "Received bytes length in this event: %zu", read_bytes);

                        // 发送接收到的数据回UART
                        // 发送接收到的数据回UART
                        int written = uart_write_bytes(UART_NUM, (const uint8_t *)dtmp, read_bytes);
                        if (written != read_bytes) {
                            ESP_LOGE(GATTS_TABLE_TAG, "Failed to write all bytes: %zu of %zu", written, read_bytes);
                        } else {
                            ESP_LOGI(GATTS_TABLE_TAG, "Successfully wrote %zu bytes back to UART", written);
                        }

                        // 重置 rx_len 以便为下一次接收做准备
                        xSemaphoreTake(uart_mutex, portMAX_DELAY);
                        rx_len = 0;
                        xSemaphoreGive(uart_mutex);
                    }
                    break;
                default:
                    break;
            }
        }
    }

    vTaskDelete(NULL); // 不要在这里释放 dtmp，因为它已经是静态变量
}




// 串口初始化函数
static void uart_init(void) {
    const uart_config_t uart_config = {
        .baud_rate = UART_BAUD_RATE,         // 波特率（115200）
        .data_bits = UART_DATA_8_BITS,       // 数据位（8位，兼容大多数设备）
        .parity = UART_PARITY_DISABLE,       // 无奇偶校验
        .stop_bits = UART_STOP_BITS_1,       // 停止位（1位，标准配置）
        .flow_ctrl = UART_HW_FLOWCTRL_DISABLE, // 禁用硬件流控（RTS/CTS）
        .source_clk = UART_SCLK_DEFAULT,     // 时钟源（APB 时钟，通常为 80MHz）
    };
    
    // 配置 UART 参数
    
    ESP_ERROR_CHECK(uart_param_config(UART_NUM, &uart_config));
    ESP_ERROR_CHECK(uart_set_pin(UART_NUM, UART_TXD, UART_RXD, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));

    // 设置 UART 事件队列
    uart_event_queue = xQueueCreate(10, sizeof(uart_event_t)); //队列最大长度为10
    
    ESP_ERROR_CHECK(uart_driver_install(UART_NUM, BUFFER_SIZE * 2, BUFFER_SIZE * 2, 10, &uart_event_queue, 0));

    ESP_ERROR_CHECK(uart_enable_pattern_det_baud_intr(UART_NUM, '\n', 1, 10, 0, 0));

    // 创建互斥锁
    // uart_mutex = xSemaphoreCreateMutex();

    // if (uart_mutex == NULL) {
    //     ESP_LOGE(GATTS_TABLE_TAG, "Failed to create UART mutex");
    //     return;
    // }

    // 测试串口初始化是否成功
    ESP_LOGI(GATTS_TABLE_TAG, "UART initialized successfully");

    // 创建串口接收任务
    xTaskCreate(uart_event_task, "uart_event_task", 4096, NULL, 12, NULL);
}


// 检测特定指令并转发到串口2
void handle_special_command(uint8_t *data, size_t len) {
    // 特定指令前缀和长度检查
    if (len >= 20 && memcmp(data, "\x5A\xAA\x00\x03\x00\x0C\x00\x00\x00\x00", 10) == 0) {

        // 打印数据为十六进制格式
        ESP_LOGI(GATTS_TABLE_TAG, "Data received:");
        print_hex(data, len);

        // 确保不会溢出发送缓冲区
        if (len > BUFFER_SIZE) {
            ESP_LOGW(GATTS_TABLE_TAG, "Data too large for buffer, truncating...");
            len = BUFFER_SIZE;
        }

        // 使用互斥锁保护对发送缓冲区的操作
        xSemaphoreTake(uart_mutex, portMAX_DELAY);
        memcpy(tx_buffer, data, len);
        xSemaphoreGive(uart_mutex);

        // 将命令发送到串口2
        int written = uart_write_bytes(UART_NUM, (const uint8_t *)tx_buffer, len);
        if (written != len) {
            ESP_LOGE(GATTS_TABLE_TAG, "Failed to write all bytes: %d of %zu", written, len);
        } else {
            ESP_LOGI(GATTS_TABLE_TAG, "Successfully wrote %zu bytes to UART", len);
        }

        vTaskDelay(20 / portTICK_PERIOD_MS); // 延迟20毫秒

        // 添加换行符
        const char newline[] = "\r\n";  // 使用 \r\n 确保兼容性
        uart_write_bytes(UART_NUM, newline, sizeof(newline) - 1);  // 不包括字符串结束符 '\0'
    }
}

//变量区
volatile int blink_interval = 1;

static uint8_t adv_config_done       = 0;

uint16_t heart_rate_handle_table[HRS_IDX_NB];

typedef struct {
    uint8_t                 *prepare_buf;
    int                     prepare_len;
} prepare_type_env_t;

static prepare_type_env_t prepare_write_env;

//#define CONFIG_SET_RAW_ADV_DATA
#ifdef CONFIG_SET_RAW_ADV_DATA
static uint8_t raw_adv_data[] = {
        /* flags */
        0x02, 0x01, 0x06,
        /* tx power*/
        0x02, 0x0a, 0xeb,
        /* service uuid */
        0x03, 0x03, 0xFF, 0x00,
        /* device name */
        0x0f, 0x09, 'E', 'S', 'P', '_', 'G', 'A', 'T', 'T', 'S', '_', 'D','E', 'M', 'O'
};
static uint8_t raw_scan_rsp_data[] = {
        /* flags */
        0x02, 0x01, 0x06,
        /* tx power */
        0x02, 0x0a, 0xeb,
        /* service uuid */
        0x03, 0x03, 0xFF,0x00
};

#else
static uint8_t service_uuid[16] = {
    /* LSB <--------------------------------------------------------------------------------> MSB */
    //first uuid, 16bit, [12],[13] is the value
    0xfb, 0x34, 0x9b, 0x5f, 0x80, 0x00, 0x00, 0x80, 0x00, 0x10, 0x00, 0x00, 0xFF, 0x00, 0x00, 0x00,
};

/* The length of adv data must be less than 31 bytes */
static esp_ble_adv_data_t adv_data = {
    .set_scan_rsp        = false,
    .include_name        = true,
    .include_txpower     = true,
    .min_interval        = 0x0006, //slave connection min interval, Time = min_interval * 1.25 msec
    .max_interval        = 0x0010, //slave connection max interval, Time = max_interval * 1.25 msec
    .appearance          = 0x00,
    .manufacturer_len    = 0,    //TEST_MANUFACTURER_DATA_LEN,
    .p_manufacturer_data = NULL, //test_manufacturer,
    .service_data_len    = 0,
    .p_service_data      = NULL,
    .service_uuid_len    = sizeof(service_uuid),
    .p_service_uuid      = service_uuid,
    .flag = (ESP_BLE_ADV_FLAG_GEN_DISC | ESP_BLE_ADV_FLAG_BREDR_NOT_SPT),
};

// scan response data
static esp_ble_adv_data_t scan_rsp_data = {
    .set_scan_rsp        = true,
    .include_name        = true,
    .include_txpower     = true,
    .min_interval        = 0x0006,
    .max_interval        = 0x0010,
    .appearance          = 0x00,
    .manufacturer_len    = 0, //TEST_MANUFACTURER_DATA_LEN,
    .p_manufacturer_data = NULL, //&test_manufacturer[0],
    .service_data_len    = 0,
    .p_service_data      = NULL,
    .service_uuid_len    = sizeof(service_uuid),
    .p_service_uuid      = service_uuid,
    .flag = (ESP_BLE_ADV_FLAG_GEN_DISC | ESP_BLE_ADV_FLAG_BREDR_NOT_SPT),
};
#endif /* CONFIG_SET_RAW_ADV_DATA */

static esp_ble_adv_params_t adv_params = {
    .adv_int_min         = 0x20,
    .adv_int_max         = 0x40,
    .adv_type            = ADV_TYPE_IND,
    .own_addr_type       = BLE_ADDR_TYPE_PUBLIC,
    .channel_map         = ADV_CHNL_ALL,
    .adv_filter_policy   = ADV_FILTER_ALLOW_SCAN_ANY_CON_ANY,
};
static const char *TAG = "JUNGT_OUT_TASK";
struct gatts_profile_inst {
    esp_gatts_cb_t gatts_cb;
    uint16_t gatts_if;
    uint16_t app_id;
    uint16_t conn_id;
    uint16_t service_handle;
    esp_gatt_srvc_id_t service_id;
    uint16_t char_handle;
    esp_bt_uuid_t char_uuid;
    esp_gatt_perm_t perm;
    esp_gatt_char_prop_t property;
    uint16_t descr_handle;
    esp_bt_uuid_t descr_uuid;
};

static void gatts_profile_event_handler(esp_gatts_cb_event_t event,
					esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t *param);

/* One gatt-based profile one app_id and one gatts_if, this array will store the gatts_if returned by ESP_GATTS_REG_EVT */
static struct gatts_profile_inst heart_rate_profile_tab[PROFILE_NUM] = {
    [PROFILE_APP_IDX] = {
        .gatts_cb = gatts_profile_event_handler,
        .gatts_if = ESP_GATT_IF_NONE,       /* Not get the gatt_if, so initial is ESP_GATT_IF_NONE */
    },
};

/* Service */
static const uint16_t GATTS_SERVICE_UUID_TEST      = 0x00FF;
static const uint16_t GATTS_CHAR_UUID_TEST_JUNGT_OUT        = 0xFF01; //APP 输出 
static const uint16_t GATTS_CHAR_UUID_TEST_JUNGT_IN        = 0xFF02;    //APP接收

static const uint16_t primary_service_uuid         = ESP_GATT_UUID_PRI_SERVICE;
static const uint16_t character_declaration_uuid   = ESP_GATT_UUID_CHAR_DECLARE;
static const uint16_t character_client_config_uuid = ESP_GATT_UUID_CHAR_CLIENT_CONFIG;
static const uint8_t char_prop_read_write_notify   = ESP_GATT_CHAR_PROP_BIT_WRITE | ESP_GATT_CHAR_PROP_BIT_READ | ESP_GATT_CHAR_PROP_BIT_NOTIFY;
static const uint8_t heart_measurement_ccc[2]      = {0x00, 0x00};

static const uint8_t JUNGT_out_value[20]                 = {0};
static const uint8_t JUNGT_IN_value[20]                 = {0};


TaskHandle_t *pTask = NULL;
volatile bool notify_flag = false;


/* Full Database Description - Used to add attributes into the database */
static const esp_gatts_attr_db_t gatt_db[HRS_IDX_NB] = {
    // Service Declaration
    [IDX_SVC]        =
    {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&primary_service_uuid, ESP_GATT_PERM_READ,
      sizeof(uint16_t), sizeof(GATTS_SERVICE_UUID_TEST), (uint8_t *)&GATTS_SERVICE_UUID_TEST}},

//-------------------------- 自定义   -----------------------------------------------------------------------------------
    /* Characteristic Declaration */
    [IDX_CHAR_JUNGT_OUT]      =
    {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_declaration_uuid, ESP_GATT_PERM_READ,
      CHAR_DECLARATION_SIZE, CHAR_DECLARATION_SIZE, (uint8_t *)&char_prop_read_write_notify}},

    /* Characteristic Value */
    [IDX_CHAR_VAL_JUNGT_OUT]  =
    {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&GATTS_CHAR_UUID_TEST_JUNGT_OUT, ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE,
      GATTS_DEMO_CHAR_VAL_LEN_MAX, sizeof(JUNGT_out_value), (uint8_t *)JUNGT_out_value}},
//-------------------------- 自定义  -----------------------------------------------------------------------------------
    /* Characteristic Declaration */
    [IDX_CHAR_JUNGT_IN]     =
    {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_declaration_uuid, ESP_GATT_PERM_READ,
      CHAR_DECLARATION_SIZE, CHAR_DECLARATION_SIZE, (uint8_t *)&char_prop_read_write_notify}},

    /* Characteristic Value */
    [IDX_CHAR_VAL_JUNGT_IN] =
    {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&GATTS_CHAR_UUID_TEST_JUNGT_IN, ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE,
      GATTS_DEMO_CHAR_VAL_LEN_MAX, sizeof(JUNGT_IN_value), (uint8_t *)JUNGT_IN_value}},

    /* Client Characteristic Configuration Descriptor */
    [IDX_CHAR_CFG_JUNGT_IN]  =
    {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_client_config_uuid, ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE,
      sizeof(uint16_t), sizeof(heart_measurement_ccc), (uint8_t *)heart_measurement_ccc}},

};

void set_servo_angle(float angle); //设置舵机角度

//函数区
static void gap_event_handler(esp_gap_ble_cb_event_t event, esp_ble_gap_cb_param_t *param)
{
    switch (event) {
    #ifdef CONFIG_SET_RAW_ADV_DATA // 如果配置了使用原始广告数据
        case ESP_GAP_BLE_ADV_DATA_RAW_SET_COMPLETE_EVT:// 广告数据设置完成事件
            adv_config_done &= (~ADV_CONFIG_FLAG);// 清除广告配置标志
            if (adv_config_done == 0){// 如果所有配置都已完成
                esp_ble_gap_start_advertising(&adv_params);// 开始广告
            }
            break;
        case ESP_GAP_BLE_SCAN_RSP_DATA_RAW_SET_COMPLETE_EVT:// 扫描响应数据设置完成事件
            adv_config_done &= (~SCAN_RSP_CONFIG_FLAG); // 清除扫描响应配置标志
            if (adv_config_done == 0)// 如果所有配置都已完成
            {
                esp_ble_gap_start_advertising(&adv_params);// 开始广告
            }
            break;
    #else
        case ESP_GAP_BLE_ADV_DATA_SET_COMPLETE_EVT:// 广告数据设置完成事件（非原始数据）
            adv_config_done &= (~ADV_CONFIG_FLAG);// 清除广告配置标志
            if (adv_config_done == 0)// 如果所有配置都已完成
            {
                // 开始广告
                esp_ble_gap_start_advertising(&adv_params);
            }
            break;
        case ESP_GAP_BLE_SCAN_RSP_DATA_SET_COMPLETE_EVT:// 扫描响应数据设置完成事件（非原始数据）
            adv_config_done &= (~SCAN_RSP_CONFIG_FLAG);// 清除扫描响应配置标志
            if (adv_config_done == 0)// 如果所有配置都已完成
            {
                // 开始广告
                esp_ble_gap_start_advertising(&adv_params);
            }
            break;
    #endif
        case ESP_GAP_BLE_ADV_START_COMPLETE_EVT:// 广告开始完成事件
            /* advertising start complete event to indicate advertising start successfully or failed */
            if (param->adv_start_cmpl.status != ESP_BT_STATUS_SUCCESS)// 检查广告开始是否成功
            {
                ESP_LOGE(GATTS_TABLE_TAG, "advertising start failed");// 如果失败，记录错误信息
            }else{
                ESP_LOGI(GATTS_TABLE_TAG, "advertising start successfully");// 如果失败，记录错误信息
            }
            break;
        case ESP_GAP_BLE_ADV_STOP_COMPLETE_EVT:// 广告停止完成事件
            if (param->adv_stop_cmpl.status != ESP_BT_STATUS_SUCCESS) // 检查广告停止是否成功
            {
                ESP_LOGE(GATTS_TABLE_TAG, "Advertising stop failed");// 如果失败，记录错误信息
            }
            else {
                ESP_LOGI(GATTS_TABLE_TAG, "Stop adv successfully\n");// 如果成功，记录成功信息
            }
            break;
        case ESP_GAP_BLE_UPDATE_CONN_PARAMS_EVT:// 连接参数更新事件
            ESP_LOGI(GATTS_TABLE_TAG, "update connection params status = %d, min_int = %d, max_int = %d,conn_int = %d,latency = %d, timeout = %d",// 记录连接参数更新的状态和具体参数
                  param->update_conn_params.status,//   状态
                  param->update_conn_params.min_int,// 最小间隔
                  param->update_conn_params.max_int,// 最大间隔
                  param->update_conn_params.conn_int,// 连接间隔
                  param->update_conn_params.latency,// 滞后
                  param->update_conn_params.timeout);// 超时
            break;
        default:
            break;
    }
}

//处理GATT服务器写入事件
void example_prepare_write_event_env(esp_gatt_if_t gatts_if, prepare_type_env_t *prepare_write_env, esp_ble_gatts_cb_param_t *param)
{
    ESP_LOGI(GATTS_TABLE_TAG, "prepare write, handle = %d, value len = %d", param->write.handle, param->write.len);// 记录写入事件的处理
    esp_gatt_status_t status = ESP_GATT_OK;// 初始化状态为成功
    if (prepare_write_env->prepare_buf == NULL) // 如果准备写入缓冲区为空
    {
        prepare_write_env->prepare_buf = (uint8_t *)malloc(PREPARE_BUF_MAX_SIZE * sizeof(uint8_t));// 分配准备写入缓冲区
        prepare_write_env->prepare_len = 0;// 初始化准备写入缓冲区长度为0
        if (prepare_write_env->prepare_buf == NULL)// 如果分配失败
        {
            ESP_LOGE(GATTS_TABLE_TAG, "%s, Gatt_server prep no mem", __func__);// 记录错误信息
            status = ESP_GATT_NO_RESOURCES;// 设置状态为无资源
        }
    } else {
        if(param->write.offset > PREPARE_BUF_MAX_SIZE) // 如果写入偏移量大于准备写入缓冲区大小
        {
            status = ESP_GATT_INVALID_OFFSET;// 设置状态为无效偏移量
        } else if ((param->write.offset + param->write.len) > PREPARE_BUF_MAX_SIZE) // 如果写入偏移量加上写入长度大于准备写入缓冲区大小
        {
            status = ESP_GATT_INVALID_ATTR_LEN;// 设置状态为无效属性长度
        }
    }
    /*send response when param->write.need_rsp is true */
    if (param->write.need_rsp)// 如果需要响应
    {
        esp_gatt_rsp_t *gatt_rsp = (esp_gatt_rsp_t *)malloc(sizeof(esp_gatt_rsp_t));// 分配响应缓冲区
        if (gatt_rsp != NULL)// 如果分配成功
        {
            gatt_rsp->attr_value.len = param->write.len;// 设置响应长度为写入长度
            gatt_rsp->attr_value.handle = param->write.handle;// 设置响应句柄为写入句柄
            gatt_rsp->attr_value.offset = param->write.offset;// 设置响应偏移量为写入偏移量
            gatt_rsp->attr_value.auth_req = ESP_GATT_AUTH_REQ_NONE;// 设置响应认证要求为无
            memcpy(gatt_rsp->attr_value.value, param->write.value, param->write.len);// 复制写入值到响应值
            esp_err_t response_err = esp_ble_gatts_send_response(gatts_if, param->write.conn_id, param->write.trans_id, status, gatt_rsp);// 发送响应
            if (response_err != ESP_OK)// 如果发送响应失败
            {
               ESP_LOGE(GATTS_TABLE_TAG, "Send response error");// 记录错误信息
            }
            free(gatt_rsp);// 释放响应缓冲区
        }else{
            ESP_LOGE(GATTS_TABLE_TAG, "%s, malloc failed", __func__);// 记录错误信息
        }
    }
    if (status != ESP_GATT_OK)// 如果状态不是成功
    {
        return;// 退出函数
    }
    memcpy(prepare_write_env->prepare_buf + param->write.offset,// 将写入值复制到准备写入缓冲区
           param->write.value,// 从写入值开始
           param->write.len);// 复制写入长度个字节
    prepare_write_env->prepare_len += param->write.len;// 更新准备写入缓冲区长度

}

void example_exec_write_event_env(prepare_type_env_t *prepare_write_env, esp_ble_gatts_cb_param_t *param)
{
    // 检查执行写操作标志
    if (param->exec_write.exec_write_flag == ESP_GATT_PREP_WRITE_EXEC && prepare_write_env->prepare_buf)
    {
        // 如果执行写操作并且准备缓冲区不为空
        // 打印准备缓冲区的内容（以十六进制格式）
        esp_log_buffer_hex(GATTS_TABLE_TAG, prepare_write_env->prepare_buf, prepare_write_env->prepare_len);
    }else{
        // 如果写操作被取消
        // 打印信息表示写操作被取消
        ESP_LOGI(GATTS_TABLE_TAG,"ESP_GATT_PREP_WRITE_CANCEL");
    }
    // 释放准备缓冲区
    if (prepare_write_env->prepare_buf) {
        free(prepare_write_env->prepare_buf);// 释放内存
        prepare_write_env->prepare_buf = NULL;// 将指针置为NULL，避免悬挂指针
    }
    // 重置准备长度
    prepare_write_env->prepare_len = 0;
}

// 传感器引脚配置
void sensor_init(void) {
    // 配置限位开关引脚为输入，并启用内部上拉
    // gpio_pad_select_gpio(LIMIT_SWITCH_1_PIN);
    // gpio_pad_select_gpio(LIMIT_SWITCH_2_PIN);
    // gpio_set_direction(LIMIT_SWITCH_1_PIN, GPIO_MODE_INPUT);
    // gpio_set_direction(LIMIT_SWITCH_2_PIN, GPIO_MODE_INPUT);
    // gpio_set_pull_mode(LIMIT_SWITCH_1_PIN, GPIO_PULLUP_ONLY);
    // gpio_set_pull_mode(LIMIT_SWITCH_2_PIN, GPIO_PULLUP_ONLY);

    // // 配置电机控制引脚为输出
    // gpio_pad_select_gpio(MOTOR_CONTROL_PIN_1);
    // gpio_pad_select_gpio(MOTOR_CONTROL_PIN_2);
    // gpio_set_direction(MOTOR_CONTROL_PIN_1, GPIO_MODE_OUTPUT);
    // gpio_set_direction(MOTOR_CONTROL_PIN_2, GPIO_MODE_OUTPUT);

    // // 配置 LED 引脚为输出
    // gpio_pad_select_gpio(LED_PIN);
    // gpio_set_direction(LED_PIN, GPIO_MODE_OUTPUT);

    // // 配置限位开关控制引脚为输出
    // gpio_pad_select_gpio(xianwei_conctrl_pin);
    // gpio_set_direction(xianwei_conctrl_pin, GPIO_MODE_INPUT);
    // gpio_set_pull_mode(xianwei_conctrl_pin, GPIO_PULLUP_ONLY);//启用内部上拉
}

uint8_t read_limit_switch_state(int pin) {
    return gpio_get_level(pin) == 0;
}



static void get_JUNGT_IN(void *arg) {
    uint8_t buff[6] = {0};  // 定义一个5字节的缓冲区，用于存储要发送的数据

    while (1) {
        if (notify_flag == true) {  // 检查是否启用了通知标志
            // 读取限位开关引脚状态
            // uint8_t limit_switch_1_state = gpio_get_level(LIMIT_SWITCH_1_PIN);
            // uint8_t limit_switch_2_state = gpio_get_level(LIMIT_SWITCH_2_PIN);
            // uint8_t switch_kaiguan_state = !gpio_get_level(xianwei_conctrl_pin);

            // // 读取电机控制引脚状态
            // // uint8_t motor_control_1_state = get_limit_switch_1_state;
            // // uint8_t motor_control_2_state = get_limit_switch_2_state;
            // uint8_t motor_last_state = get_motor_last_state;
            // uint8_t motor_new_state = get_motor_new_state;

            // 将状态放入缓冲区
            buff[0] = 0; // 限位开关1状态 0吸合 1关闭
            buff[1] = 0; // 限位开关2状态 0吸合 1关闭
            buff[2] = 0; // 电机上次状态 0: 不吸合，1: 正转，2: 反转
            buff[3] = 0; // 电机当前状态 0: 不吸合，1: 正转，2: 反转
            buff[4] = 0; // 0 表示关闭 1表示打开

            // 设置GATT服务中JUNGT输入特征值的属性值
            esp_ble_gatts_set_attr_value(heart_rate_handle_table[IDX_CHAR_JUNGT_IN], 4, buff);


            // 设置GATT服务中JUNGT输入特征值的属性值
            esp_ble_gatts_set_attr_value(heart_rate_handle_table[IDX_CHAR_JUNGT_IN], 4, buff);

            // 发送指示（Indication）给客户端
            esp_ble_gatts_send_indicate(heart_rate_profile_tab[0].gatts_if,  // GATT接口
                                        heart_rate_profile_tab[0].conn_id,   // 连接ID
                                        heart_rate_handle_table[IDX_CHAR_VAL_JUNGT_IN],  // 特征值句柄
                                        5,  // 数据长度
                                        buff,  // 数据缓冲区
                                        false);  // 是否需要确认
        } else {
            vTaskDelete(NULL);  // 如果通知标志为false，则删除当前任务
        }

        vTaskDelay(pdMS_TO_TICKS(10));  // 延迟10ms
    }
}


static void gatts_profile_event_handler(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t *param)
{
    switch (event) {
        case ESP_GATTS_REG_EVT:// GATT服务注册事件
        { 
            // 设置设备名称
            esp_err_t set_dev_name_ret = esp_ble_gap_set_device_name(SAMPLE_DEVICE_NAME);
            if (set_dev_name_ret){
                ESP_LOGE(GATTS_TABLE_TAG, "set device name failed, error code = %x", set_dev_name_ret);
            }
    #ifdef CONFIG_SET_RAW_ADV_DATA // 如果配置了原始广告数据
            // 配置原始广告数据
            esp_err_t raw_adv_ret = esp_ble_gap_config_adv_data_raw(raw_adv_data, sizeof(raw_adv_data));
            if (raw_adv_ret){
                ESP_LOGE(GATTS_TABLE_TAG, "config raw adv data failed, error code = %x ", raw_adv_ret);
            }
            adv_config_done |= ADV_CONFIG_FLAG;
            // 配置扫描响应数据
            esp_err_t raw_scan_ret = esp_ble_gap_config_scan_rsp_data_raw(raw_scan_rsp_data, sizeof(raw_scan_rsp_data));
            if (raw_scan_ret){
                ESP_LOGE(GATTS_TABLE_TAG, "config raw scan rsp data failed, error code = %x", raw_scan_ret);
            }
            adv_config_done |= SCAN_RSP_CONFIG_FLAG;
    #else   // 如果没有配置原始广告数据
            // 配置标准广告数据
            esp_err_t ret = esp_ble_gap_config_adv_data(&adv_data);
            if (ret){
                ESP_LOGE(GATTS_TABLE_TAG, "config adv data failed, error code = %x", ret);
            }
            adv_config_done |= ADV_CONFIG_FLAG;
            // 配置扫描响应数据
            ret = esp_ble_gap_config_adv_data(&scan_rsp_data);
            if (ret){
                ESP_LOGE(GATTS_TABLE_TAG, "config scan response data failed, error code = %x", ret);
            }
            adv_config_done |= SCAN_RSP_CONFIG_FLAG;
    #endif
            // 创建属性表
            esp_err_t create_attr_ret = esp_ble_gatts_create_attr_tab(gatt_db, gatts_if, HRS_IDX_NB, SVC_INST_ID);
            if (create_attr_ret){
                ESP_LOGE(GATTS_TABLE_TAG, "create attr table failed, error code = %x", create_attr_ret);
            }
        }
       	    break;
        case ESP_GATTS_READ_EVT: // 读取事件
            ESP_LOGI(GATTS_TABLE_TAG, "ESP_GATTS_READ_EVT");
       	    break;
        case ESP_GATTS_WRITE_EVT:// 写入事件
            if (!param->write.is_prep){ // 如果不是准备写入
                // the data length of gattc write  must be less than GATTS_DEMO_CHAR_VAL_LEN_MAX.// 检查写入数据长度
                ESP_LOGI(GATTS_TABLE_TAG, "GATT_WRITE_EVT, handle = %d, value len = %d, value :", param->write.handle, param->write.len);
                esp_log_buffer_hex(GATTS_TABLE_TAG, param->write.value, param->write.len);
                // 处理不同特征值的写入
                if(heart_rate_handle_table[IDX_CHAR_VAL_JUNGT_OUT] == param->write.handle){
                // JUNGT输出特征值的写入处理
                    esp_log_buffer_hex(GATTS_TABLE_TAG,param->write.value,param->write.len);
                    if (param->write.len >= 3) {//至少需要3字节数据
                        if (param->write.value[0] == 0x01 ) {
                            //提取后两位作为舵机角度
                            float angle = param->write.value[1];
                            float angle2 = param->write.value[2];
                            float aim_angle = angle + angle2/100;
                            if (angle <= 180) {//检查角度是否在有效范围内
                                ESP_LOGI(TAG, "Received 0x01, Setting servo angle to: %lf", aim_angle);
                                
                                set_servo_angle(aim_angle); // 设置舵机角度
                            }else{
                                ESP_LOGE(TAG, "Invalid angle: %lf (must be 0~180)", aim_angle);
                            }
                    }else if (param->write.value[0] == 0x5A ) {
                        // 检测特殊指令并转发到串口2
                        // 特定指令前缀和长度检查
                        u16_t len = param->write.len;
                        uint8_t data[len];
                        ESP_LOGI(GATTS_TABLE_TAG, "Data received: 5A");
                        if (param->write.len >= 10 ) {
                            // 打印数据为十六进制格式
                            memcpy(data, param->write.value, len);
                            len = param->write.len;
                            ESP_LOGI(GATTS_TABLE_TAG, "Data received:");
                            print_hex(data, len);

                            // 确保不会溢出发送缓冲区
                            if (len > BUFFER_SIZE) {
                                ESP_LOGW(GATTS_TABLE_TAG, "Data too large for buffer, truncating...");
                                len = BUFFER_SIZE;
                            }

                            // 使用互斥锁保护对发送缓冲区的操作
                            xSemaphoreTake(uart_mutex, portMAX_DELAY);
                            memcpy(tx_buffer, data, len);
                            xSemaphoreGive(uart_mutex);

                            // 将命令发送到串口2
                            int written = uart_write_bytes(UART_NUM, (const uint8_t *)tx_buffer, len);
                            if (written != len) {
                                ESP_LOGE(GATTS_TABLE_TAG, "Failed to write all bytes: %d of %zu", written, len);
                            } else {
                                ESP_LOGI(GATTS_TABLE_TAG, "Successfully wrote %zu bytes to UART", len);
                            }

                            vTaskDelay(20 / portTICK_PERIOD_MS); // 延迟20毫秒
                        }
                        // handle_special_command(param->write.value, param->write.len);
                    }
                    }
                    


                    notify_flag = true; // 启用通知
                    ESP_LOGI(GATTS_TABLE_TAG, "notify enable");

                    // 如果需要响应，则发送响应
                    if (param->write.need_rsp) {
                        esp_ble_gatts_send_response(gatts_if, param->write.conn_id, param->write.trans_id, ESP_GATT_OK, NULL);
                    }
                }else if(heart_rate_handle_table[IDX_CHAR_CFG_JUNGT_IN] == param->write.handle){
                    // JUNGT输入特征值的写入处理
                    esp_log_buffer_hex(GATTS_TABLE_TAG,param->write.value,param->write.len);
                    notify_flag = true ;// 启用通知
                    ESP_LOGI(GATTS_TABLE_TAG, "notify enable");
                    xTaskCreate(get_JUNGT_IN, "get temp", 8192, NULL, 10, pTask);// 创建任务获取JUNGT输入特征值
                }
                /* send response when param->write.need_rsp is true*/ // 如果需要响应，则发送响应
                if (param->write.need_rsp){
                    esp_ble_gatts_send_response(gatts_if, param->write.conn_id, param->write.trans_id, ESP_GATT_OK, NULL);
                }
            }else{
                // 处理准备写入
                /* handle prepare write */
                example_prepare_write_event_env(gatts_if, &prepare_write_env, param);
            }
      	    break;
        case ESP_GATTS_EXEC_WRITE_EVT:// 执行写入事件
            // the length of gattc prepare write data must be less than GATTS_DEMO_CHAR_VAL_LEN_MAX.
            ESP_LOGI(GATTS_TABLE_TAG, "ESP_GATTS_EXEC_WRITE_EVT");
            example_exec_write_event_env(&prepare_write_env, param);
            break;
        case ESP_GATTS_MTU_EVT:// MTU交换事件
            ESP_LOGI(GATTS_TABLE_TAG, "ESP_GATTS_MTU_EVT, MTU %d", param->mtu.mtu);
            break;
        case ESP_GATTS_CONF_EVT: // 确认事件
            ESP_LOGI(GATTS_TABLE_TAG, "ESP_GATTS_CONF_EVT, status = %d, attr_handle %d", param->conf.status, param->conf.handle);
            break;
        case ESP_GATTS_START_EVT:// 服务启动事件
            ESP_LOGI(GATTS_TABLE_TAG, "SERVICE_START_EVT, status %d, service_handle %d", param->start.status, param->start.service_handle);
            break;
        case ESP_GATTS_CONNECT_EVT:// 服务启动事件
            ESP_LOGI(GATTS_TABLE_TAG, "ESP_GATTS_CONNECT_EVT, conn_id = %d", param->connect.conn_id);
            esp_log_buffer_hex(GATTS_TABLE_TAG, param->connect.remote_bda, 6);

             // 更新连接参数
            esp_ble_conn_update_params_t conn_params = {0};
            memcpy(conn_params.bda, param->connect.remote_bda, sizeof(esp_bd_addr_t));
            /* For the iOS system, please refer to Apple official documents about the BLE connection parameters restrictions. */
            conn_params.latency = 0;
            conn_params.max_int = 0x20;    // max_int = 0x20*1.25ms = 40ms // 最大间隔 40ms
            conn_params.min_int = 0x10;    // min_int = 0x10*1.25ms = 20ms // 最小间隔 20ms
            conn_params.timeout = 400;    // timeout = 400*10ms = 4000ms // 超时 4000ms
            //start sent the update connection parameters to the peer device.
            esp_ble_gap_update_conn_params(&conn_params);
            break;
        case ESP_GATTS_DISCONNECT_EVT: // 断开连接事件
            ESP_LOGI(GATTS_TABLE_TAG, "ESP_GATTS_DISCONNECT_EVT, reason = 0x%x", param->disconnect.reason);
            esp_ble_gap_start_advertising(&adv_params);// 开始广告
            notify_flag = false;// 禁用通知
            break;
        case ESP_GATTS_CREAT_ATTR_TAB_EVT:// 属性表创建事件
        {
            if (param->add_attr_tab.status != ESP_GATT_OK){
                ESP_LOGE(GATTS_TABLE_TAG, "create attribute table failed, error code=0x%x", param->add_attr_tab.status);
            }
            else if (param->add_attr_tab.num_handle != HRS_IDX_NB){
                ESP_LOGE(GATTS_TABLE_TAG, "create attribute table abnormally, num_handle (%d) \
                        doesn't equal to HRS_IDX_NB(%d)", param->add_attr_tab.num_handle, HRS_IDX_NB);
            }
            else {
                ESP_LOGI(GATTS_TABLE_TAG, "create attribute table successfully, the number handle = %d\n",param->add_attr_tab.num_handle);
                memcpy(heart_rate_handle_table, param->add_attr_tab.handles, sizeof(heart_rate_handle_table));
                esp_ble_gatts_start_service(heart_rate_handle_table[IDX_SVC]);
            }
            break;
        }
        case ESP_GATTS_STOP_EVT:
        case ESP_GATTS_OPEN_EVT:
        case ESP_GATTS_CANCEL_OPEN_EVT:
        case ESP_GATTS_CLOSE_EVT:
        case ESP_GATTS_LISTEN_EVT:
        case ESP_GATTS_CONGEST_EVT:
        case ESP_GATTS_UNREG_EVT:
        case ESP_GATTS_DELETE_EVT:
        default:
            // 其他事件或默认情况
            break;
    }
}


static void gatts_event_handler(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t *param)
{

    /* If event is register event, store the gatts_if for each profile */
    if (event == ESP_GATTS_REG_EVT) {
        if (param->reg.status == ESP_GATT_OK) {
            heart_rate_profile_tab[PROFILE_APP_IDX].gatts_if = gatts_if;
        } else {
            ESP_LOGE(GATTS_TABLE_TAG, "reg app failed, app_id %04x, status %d",
                    param->reg.app_id,
                    param->reg.status);
            return;
        }
    }
    do {
        int idx;
        for (idx = 0; idx < PROFILE_NUM; idx++) {
            /* ESP_GATT_IF_NONE, not specify a certain gatt_if, need to call every profile cb function */
            if (gatts_if == ESP_GATT_IF_NONE || gatts_if == heart_rate_profile_tab[idx].gatts_if) {
                if (heart_rate_profile_tab[idx].gatts_cb) {
                    heart_rate_profile_tab[idx].gatts_cb(event, gatts_if, param);
                }
            }
        }
    } while (0);
}

// 舵机配置
// 初始化 LEDC PWM
void setup_ledc() {
    // 配置 PWM 定时器
    ledc_timer_config_t timer_conf = {
        .speed_mode = LEDC_MODE,
        .duty_resolution = LEDC_DUTY_RES,
        .timer_num = LEDC_TIMER,
        .freq_hz = LEDC_FREQUENCY,
        .clk_cfg = LEDC_AUTO_CLK
    };
    ledc_timer_config(&timer_conf);

    // 配置舵机 1 的 PWM 通道
    ledc_channel_config_t channel1_conf = {
        .gpio_num = SERVO1_GPIO,
        .speed_mode = LEDC_MODE,
        .channel = LEDC_CHANNEL1,
        .timer_sel = LEDC_TIMER,
        .duty = 0, // 初始占空比
        .hpoint = 0
    };
    ledc_channel_config(&channel1_conf);

    // 配置舵机 2 的 PWM 通道
    ledc_channel_config_t channel2_conf = {
        .gpio_num = SERVO2_GPIO,
        .speed_mode = LEDC_MODE,
        .channel = LEDC_CHANNEL2,
        .timer_sel = LEDC_TIMER,
        .duty = 0, // 初始占空比
        .hpoint = 0
    };

    // 应用通道2的配置
    ledc_channel_config(&channel2_conf);

    ESP_LOGI(TAG, "LEDC PWM initialized for two servos with 16-bit resolution");
}

// 设置舵机角度
void set_servo_angle(float angle) {
    // 限制角度范围
    if (angle < SERVO_MIN_ANGLE) angle = SERVO_MIN_ANGLE;
    if (angle > SERVO_MAX_ANGLE) angle = SERVO_MAX_ANGLE;

    // 计算舵机 1 的 PWM 值（正转）
    int pwm_value1 = SERVO_MIN_DUTY + (int)(angle * (SERVO_MAX_DUTY - SERVO_MIN_DUTY) / SERVO_MAX_ANGLE);

    // 计算舵机 2 的 PWM 值（反转）
    int pwm_value2 = SERVO_MIN_DUTY + (int)((SERVO_MAX_ANGLE - angle) * (SERVO_MAX_DUTY - SERVO_MIN_DUTY) / SERVO_MAX_ANGLE);

    // 设置舵机 1 的 PWM 占空比
    ledc_set_duty(LEDC_MODE, LEDC_CHANNEL1, pwm_value1);
    ledc_update_duty(LEDC_MODE, LEDC_CHANNEL1);

    // 设置舵机 2 的 PWM 占空比
    ledc_set_duty(LEDC_MODE, LEDC_CHANNEL2, pwm_value2);
    ledc_update_duty(LEDC_MODE, LEDC_CHANNEL2);

    ESP_LOGI(TAG, "Setting servo 1 angle to: %.1f°, PWM value: %d", angle, pwm_value1);
    ESP_LOGI(TAG, "Setting servo 2 angle to: %.1f°, PWM value: %d", (180.0f - angle), pwm_value2);
}

// 缓慢移动到目标角度
void move_servo_slowly(float target_angle) {
    static float current_angle = 0.0f; // 当前角度

    if (target_angle > current_angle) {
        // 逐步增加角度
        for (float angle = current_angle; angle <= target_angle; angle += STEP_SIZE) {
            set_servo_angle(angle);
            vTaskDelay(STEP_DELAY_MS / portTICK_PERIOD_MS);
        }
    } else {
        // 逐步减少角度
        for (float angle = current_angle; angle >= target_angle; angle -= STEP_SIZE) {
            set_servo_angle(angle);
            vTaskDelay(STEP_DELAY_MS / portTICK_PERIOD_MS);
        }
    }

    current_angle = target_angle; // 更新当前角度
}


void app_main(void)
{
    sensor_init();
    setup_ledc();
    uart_init();

    esp_err_t ret;// 定义错误返回值变量

    /* Initialize NVS. */
    ret = nvs_flash_init(); // 初始化NVS，这是用于存储少量数据的非易失性存储系统
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) { // 如果NVS中没有可用页面或发现新版本，则擦除NVS分区并重新初始化
        ESP_ERROR_CHECK(nvs_flash_erase());
        ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK( ret );// 检查NVS初始化是否成功

    ESP_ERROR_CHECK(esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT)); // 释放经典蓝牙模式下的控制器内存，因为我们只使用BLE

    esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT(); // 设置默认的蓝牙控制器配置
    ret = esp_bt_controller_init(&bt_cfg); // 使用上述配置初始化蓝牙控制器
    if (ret) { // 如果初始化失败，打印错误信息并退出
        ESP_LOGE(GATTS_TABLE_TAG, "%s enable controller failed: %s", __func__, esp_err_to_name(ret));
        return;
    }

    ret = esp_bt_controller_enable(ESP_BT_MODE_BLE); // 启用BLE模式
    if (ret) { // 如果启用失败，打印错误信息并退出
        ESP_LOGE(GATTS_TABLE_TAG, "%s enable controller failed: %s", __func__, esp_err_to_name(ret));
        return;
    }

    ret = esp_bluedroid_init(); // 初始化Bluedroid蓝牙协议栈
    if (ret) { // 如果初始化失败，打印错误信息并退出
        ESP_LOGE(GATTS_TABLE_TAG, "%s init bluetooth failed: %s", __func__, esp_err_to_name(ret));
        return;
    }

    ret = esp_bluedroid_enable(); // 启用Bluedroid蓝牙协议栈
    if (ret) { // 如果启用失败，打印错误信息并退出
        ESP_LOGE(GATTS_TABLE_TAG, "%s enable bluetooth failed: %s", __func__, esp_err_to_name(ret));
        return;
    }

     // 注册GATT服务器事件处理函数
    ret = esp_ble_gatts_register_callback(gatts_event_handler);
    if (ret){
        ESP_LOGE(GATTS_TABLE_TAG, "gatts register error, error code = %x", ret);
        return;
    }

    // 注册GAP事件处理函数
    ret = esp_ble_gap_register_callback(gap_event_handler);
    if (ret){
        ESP_LOGE(GATTS_TABLE_TAG, "gap register error, error code = %x", ret);
        return;
    }

     // 注册GATT应用
    ret = esp_ble_gatts_app_register(ESP_APP_ID);
    if (ret){
        ESP_LOGE(GATTS_TABLE_TAG, "gatts app register error, error code = %x", ret);
        return;
    }
    
    // 设置本地MTU大小
    esp_err_t local_mtu_ret = esp_ble_gatt_set_local_mtu(500);
    if (local_mtu_ret){
        ESP_LOGE(GATTS_TABLE_TAG, "set local  MTU failed, error code = %x", local_mtu_ret);
    }

    // xTaskCreate(read_sensors_task, "read_sensors_task", 2048, NULL, 5, NULL);
    // xTaskCreate(led_blink_task, "led_blink_task", 2048, NULL, 5, NULL);
    // xTaskCreate(motor_control_task, "motor_control_task", 2048, NULL, 5, NULL);
    // xTaskCreate(uart_task, "uart_task", 4096, NULL, 5, NULL);
}

